# 电机的电感是非线性的
# 考虑到最终的目的是使用电感Ld，Lq，磁链ψ参数估计Vd、Vq，采用电压方程的方式求解
# 根据永磁同步电机foc控制的电压方程，已知ω、Vd，Vq，Id，Iq，Rs的实测数据
# 求解Ld，Lq，ψ，使用python编程实现。
# d轴电压方程：
# Vd = Rs*Id − ω*Lq*Iq
# q轴电压方程：
# Vq = Rs*Iq + ω*Ld*Id + ω*ψ

# 对于Id较小（小于0.01A）的数据，可以认为Id=0，将方程简化为Vd = − ω*Lq*Iq， Vq = Rs*Iq + ω*ψ，仅参与Lq、ψ的计算
# 然后使用剩余的其他数据，计算Lq
# 应当选择适当的拟合算法，减少计算误差，最好能自动舍弃异常数据。

# 以下是参考数据(空载测试用例)
# Rs = 2.0Ω
# ω=503.273102rad/s, Vd=0.105552V, Vq=2.500837V, Id=0.000092A, Iq=0.063475A
# ω=767.242065rad/s, Vd=0.017552V, Vq=3.691411V, Id=0.000226A, Iq=0.066944A
# ω=1030.900879rad/s, Vd=-0.113780V, Vq=4.864276V, Id=0.000034A, Iq=0.069499A
# ω=1293.886719rad/s, Vd=-0.256612V, Vq=6.040136V, Id=-0.000068A, Iq=0.072923A
# ω=1557.071533rad/s, Vd=-0.337052V, Vq=7.199516V, Id=-0.000196A, Iq=0.076901A
# ω=1821.093140rad/s, Vd=-0.556760V, Vq=8.360329V, Id=-0.000009A, Iq=0.079900A
# ω=2085.770264rad/s, Vd=-0.766832V, Vq=9.509966V, Id=0.000136A, Iq=0.085005A
# ω=2348.405029rad/s, Vd=-1.062644V, Vq=10.658028V, Id=0.000222A, Iq=0.090798A
# ω=2612.455811rad/s, Vd=-1.283611V, Vq=11.787005V, Id=-0.000236A, Iq=0.095083A
# ω=2876.465576rad/s, Vd=-1.681840V, Vq=12.919694V, Id=0.000222A, Iq=0.099631A
# ω=3140.264893rad/s, Vd=-1.991344V, Vq=14.009653V, Id=-0.000161A, Iq=0.105262A
# ω=3379.586670rad/s, Vd=-2.364431V, Vq=15.013895V, Id=-0.000391A, Iq=0.110308A
# ω=3643.374268rad/s, Vd=-2.831958V, Vq=16.113613V, Id=0.000065A, Iq=0.115682A
# ω=3905.874268rad/s, Vd=-3.148240V, Vq=22.470039V, Id=0.312853A, Iq=0.096356A
# ω=4171.645508rad/s, Vd=-3.689340V, Vq=22.388874V, Id=0.228377A, Iq=0.106992A
# ω=4433.724609rad/s, Vd=-4.226974V, Vq=22.278868V, Id=0.154287A, Iq=0.118102A
# ω=4698.992676rad/s, Vd=-4.769433V, Vq=22.160297V, Id=0.089461A, Iq=0.127110A
# ω=4960.328125rad/s, Vd=-5.377406V, Vq=22.036245V, Id=0.033801A, Iq=0.137203A
# ω=5227.445312rad/s, Vd=-5.958261V, Vq=21.869284V, Id=-0.016155A, Iq=0.146214A
# ω=5490.366699rad/s, Vd=-6.466906V, Vq=21.774994V, Id=-0.059715A, Iq=0.155630A
# ω=5752.802734rad/s, Vd=-6.976800V, Vq=21.608583V, Id=-0.099297A, Iq=0.164662A
# ω=6016.994141rad/s, Vd=-7.416429V, Vq=21.403662V, Id=-0.135583A, Iq=0.173569A
# ω=6279.324219rad/s, Vd=-7.902421V, Vq=21.287481V, Id=-0.168243A, Iq=0.182099A
# ω=6542.661621rad/s, Vd=-8.358311V, Vq=21.043179V, Id=-0.198878A, Iq=0.189794A
# ω=6782.609375rad/s, Vd=-8.736075V, Vq=20.865480V, Id=-0.224279A, Iq=0.196471A
# ω=7047.818848rad/s, Vd=-9.147591V, Vq=20.749836V, Id=-0.249958A, Iq=0.206064A
# ω=7310.084961rad/s, Vd=-9.552227V, Vq=20.485760V, Id=-0.273571A, Iq=0.211702A
# ω=7573.942383rad/s, Vd=-9.991473V, Vq=20.345743V, Id=-0.295273A, Iq=0.221005A
# ω=7837.688965rad/s, Vd=-10.380960V, Vq=20.133331V, Id=-0.315670A, Iq=0.228779A
# ω=8101.658203rad/s, Vd=-10.767109V, Vq=19.910809V, Id=-0.334205A, Iq=0.236319A
# ω=8365.580078rad/s, Vd=-11.120441V, Vq=19.673954V, Id=-0.351758A, Iq=0.244224A
# ω=8628.952148rad/s, Vd=-11.552833V, Vq=19.440451V, Id=-0.368331A, Iq=0.251120A
# ω=8892.822266rad/s, Vd=-11.855657V, Vq=19.088614V, Id=-0.383917A, Iq=0.259678A
# ω=9155.375000rad/s, Vd=-12.199574V, Vq=18.778355V, Id=-0.398568A, Iq=0.267731A
# ω=9420.274414rad/s, Vd=-12.467855V, Vq=18.428516V, Id=-0.413499A, Iq=0.274118A
# ω=9682.791992rad/s, Vd=-12.894954V, Vq=18.159098V, Id=-0.427497A, Iq=0.283549A
# ω=9946.005859rad/s, Vd=-13.136336V, Vq=17.637056V, Id=-0.443340A, Iq=0.293436A
# ω=10187.996094rad/s, Vd=-13.196839V, Vq=17.196079V, Id=-0.454809A, Iq=0.298739A
# ω=10450.601562rad/s, Vd=-13.549364V, Vq=16.794228V, Id=-0.469933A, Iq=0.307857A
# ω=10713.400391rad/s, Vd=-13.811967V, Vq=16.279373V, Id=-0.483671A, Iq=0.317884A


# 以下是带载测试用例，实际计算时用空载的就行，方便采集数据，因此以下数据不参与计算
# ω=503.214569rad/s, Vd=-0.367849V, Vq=2.833102V, Id=0.000107A, Iq=0.219106A
# ω=771.478638rad/s, Vd=-0.581625V, Vq=3.980549V, Id=0.000101A, Iq=0.198554A
# ω=1028.162231rad/s, Vd=-1.093524V, Vq=5.270306V, Id=-0.000131A, Iq=0.292093A
# ω=1292.412109rad/s, Vd=-2.366444V, Vq=6.637251V, Id=-0.000191A, Iq=0.426201A
# ω=1557.375854rad/s, Vd=-3.331999V, Vq=7.864697V, Id=0.000066A, Iq=0.512472A
# ω=1821.192627rad/s, Vd=-4.388792V, Vq=8.968808V, Id=0.000019A, Iq=0.549808A
# ω=2084.769531rad/s, Vd=-5.429272V, Vq=10.046916V, Id=0.000077A, Iq=0.590190A
# ω=2347.480225rad/s, Vd=-6.972373V, Vq=11.059287V, Id=0.000069A, Iq=0.652834A
# ω=2612.333008rad/s, Vd=-8.280272V, Vq=12.012927V, Id=0.000454A, Iq=0.699597A
# ω=2875.915527rad/s, Vd=-9.826611V, Vq=12.851287V, Id=0.000352A, Iq=0.734733A
# ω=3140.446289rad/s, Vd=-10.929262V, Vq=19.652441V, Id=0.430871A, Iq=0.748654A
# ω=3384.917480rad/s, Vd=-12.809829V, Vq=18.466076V, Id=0.274129A, Iq=0.800864A
# ω=3641.326172rad/s, Vd=-14.694221V, Vq=16.869688V, Id=0.119834A, Iq=0.848051A
# ω=3906.582520rad/s, Vd=-16.351477V, Vq=15.282285V, Id=-0.014582A, Iq=0.881533A
# ω=4169.860840rad/s, Vd=-18.239252V, Vq=12.899877V, Id=-0.164655A, Iq=0.934810A
# ω=4434.889160rad/s, Vd=-20.033499V, Vq=9.767628V, Id=-0.326561A, Iq=0.994992A
# ω=4696.804199rad/s, Vd=-21.691719V, Vq=5.290781V, Id=-0.531615A, Iq=1.048584A

# 数据分组​​：根据Id的大小将数据分为两组。Id绝对值小于0.01A的用于计算Lq和ψ，其余的用于计算Ld。
# ​​异常值过滤​​：在计算Lq和ψ时，仅保留Vd和Iq符号相反的数据点，确保Lq为正。
# ​​鲁棒估计​​：使用中位数和IQR方法排除异常值，取均值作为最终结果。
import numpy as np

# 给定参数
Rs = 2.0  # 欧姆

# 数据输入，格式：[ω, Vd, Vq, Id, Iq]
data = [
    [503.273102, 0.105552, 2.500837, 0.000092, 0.063475],
    [767.242065, 0.017552, 3.691411, 0.000226, 0.066944],
    [1030.900879, -0.113780, 4.864276, 0.000034, 0.069499],
    [1293.886719, -0.256612, 6.040136, -0.000068, 0.072923],
    [1557.071533, -0.337052, 7.199516, -0.000196, 0.076901],
    [1821.093140, -0.556760, 8.360329, -0.000009, 0.079900],
    [2085.770264, -0.766832, 9.509966, 0.000136, 0.085005],
    [2348.405029, -1.062644, 10.658028, 0.000222, 0.090798],
    [2612.455811, -1.283611, 11.787005, -0.000236, 0.095083],
    [2876.465576, -1.681840, 12.919694, 0.000222, 0.099631],
    [3140.264893, -1.991344, 14.009653, -0.000161, 0.105262],
    [3379.586670, -2.364431, 15.013895, -0.000391, 0.110308],
    [3643.374268, -2.831958, 16.113613, 0.000065, 0.115682],
    [3905.874268, -3.148240, 22.470039, 0.312853, 0.096356],
    [4171.645508, -3.689340, 22.388874, 0.228377, 0.106992],
    [4433.724609, -4.226974, 22.278868, 0.154287, 0.118102],
    [4698.992676, -4.769433, 22.160297, 0.089461, 0.127110],
    [4960.328125, -5.377406, 22.036245, 0.033801, 0.137203],
    [5227.445312, -5.958261, 21.869284, -0.016155, 0.146214],
    [5490.366699, -6.466906, 21.774994, -0.059715, 0.155630],
    [5752.802734, -6.976800, 21.608583, -0.099297, 0.164662],
    [6016.994141, -7.416429, 21.403662, -0.135583, 0.173569],
    [6279.324219, -7.902421, 21.287481, -0.168243, 0.182099],
    [6542.661621, -8.358311, 21.043179, -0.198878, 0.189794],
    [6782.609375, -8.736075, 20.865480, -0.224279, 0.196471],
    [7047.818848, -9.147591, 20.749836, -0.249958, 0.206064],
    [7310.084961, -9.552227, 20.485760, -0.273571, 0.211702],
    [7573.942383, -9.991473, 20.345743, -0.295273, 0.221005],
    [7837.688965, -10.380960, 20.133331, -0.315670, 0.228779],
    [8101.658203, -10.767109, 19.910809, -0.334205, 0.236319],
    [8365.580078, -11.120441, 19.673954, -0.351758, 0.244224],
    [8628.952148, -11.552833, 19.440451, -0.368331, 0.251120],
    [8892.822266, -11.855657, 19.088614, -0.383917, 0.259678],
    [9155.375000, -12.199574, 18.778355, -0.398568, 0.267731],
    [9420.274414, -12.467855, 18.428516, -0.413499, 0.274118],
    [9682.791992, -12.894954, 18.159098, -0.427497, 0.283549],
    [9946.005859, -13.136336, 17.637056, -0.443340, 0.293436],
    [10187.996094, -13.196839, 17.196079, -0.454809, 0.298739],
    [10450.601562, -13.549364, 16.794228, -0.469933, 0.307857],
    [10713.400391, -13.811967, 16.279373, -0.483671, 0.317884],

    # 带载，不使用
    # [503.214569,-0.367849,2.833102,0.000107,0.219106],
    # [771.478638,-0.581625,3.980549,0.000101,0.198554],
    # [1028.162231,-1.093524,5.270306,-0.000131,0.292093],
    # [1292.412109,-2.366444,6.637251,-0.000191,0.426201],
    # [1557.375854,-3.331999,7.864697,0.000066,0.512472],
    # [1821.192627,-4.388792,8.968808,0.000019,0.549808],
    # [2084.769531,-5.429272,10.046916,0.000077,0.590190],
    # [2347.480225,-6.972373,11.059287,0.000069,0.652834],
    # [2612.333008,-8.280272,12.012927,0.000454,0.699597],
    # [2875.915527,-9.826611,12.851287,0.000352,0.734733],
    # [3140.446289,-10.929262,19.652441,0.430871,0.748654],
    # [3384.917480,-12.809829,18.466076,0.274129,0.800864],
    # [3641.326172,-14.694221,16.869688,0.119834,0.848051],
    # [3906.582520,-16.351477,15.282285,-0.014582,0.881533],
    # [4169.860840,-18.239252,12.899877,-0.164655,0.934810],
    # [4434.889160,-20.033499,9.767628,-0.326561,0.994992],
    # [4696.804199,-21.691719,5.290781,-0.531615,1.048584],
]

# 分离组1和组2
group1 = [point for point in data if abs(point[3]) < 0.01]
group2 = [point for point in data if abs(point[3]) >= 0.01]

# 处理组1，计算Lq和ψ
valid_group1 = [point for point in group1 if point[1] * point[4] < 0]

lq_values = []
psi_values = []

for point in valid_group1:
    omega, vd, vq, id_, iq = point
    # 计算Lq
    lq = -vd / (omega * iq)
    lq_values.append(lq)
    # 计算ψ
    psi = (vq - Rs * iq) / omega
    psi_values.append(psi)

# 异常值处理（Lq）
if lq_values:
    lq_median = np.median(lq_values)
    lq_q1 = np.percentile(lq_values, 25)
    lq_q3 = np.percentile(lq_values, 75)
    lq_iqr = lq_q3 - lq_q1
    lq_lower = lq_q1 - 1.5 * lq_iqr
    lq_upper = lq_q3 + 1.5 * lq_iqr
    filtered_lq = [lq for lq in lq_values if lq_lower <= lq <= lq_upper]
    final_lq = np.mean(filtered_lq)
else:
    final_lq = np.nan

# 异常值处理（ψ）
if psi_values:
    psi_median = np.median(psi_values)
    psi_q1 = np.percentile(psi_values, 25)
    psi_q3 = np.percentile(psi_values, 75)
    psi_iqr = psi_q3 - psi_q1
    psi_lower = psi_q1 - 1.5 * psi_iqr
    psi_upper = psi_q3 + 1.5 * psi_iqr
    filtered_psi = [psi for psi in psi_values if psi_lower <= psi <= psi_upper]
    final_psi = np.mean(filtered_psi)
else:
    final_psi = np.nan

# 处理组2，计算Ld
ld_values = []

for point in group2:
    omega, vd, vq, id_, iq = point
    if id_ == 0:
        continue  # 避免除以零，但根据分组条件id_绝对值>=0.01，不会出现
    numerator = vq - Rs * iq - omega * final_psi
    denominator = omega * id_
    ld = numerator / denominator
    ld_values.append(ld)

# 异常值处理（Ld）
if ld_values:
    ld_median = np.median(ld_values)
    ld_q1 = np.percentile(ld_values, 25)
    ld_q3 = np.percentile(ld_values, 75)
    ld_iqr = ld_q3 - ld_q1
    ld_lower = ld_q1 - 1.5 * ld_iqr
    ld_upper = ld_q3 + 1.5 * ld_iqr
    filtered_ld = [ld for ld in ld_values if ld_lower <= ld <= ld_upper]
    final_ld = np.mean(filtered_ld) if filtered_ld else np.nan
else:
    final_ld = np.nan

# 输出结果
print(f"计算得到的参数：")
print(f"Lq = {final_lq * 1000:.3f} mH")
print(f"ψ = {final_psi:.6f} Wb")
print(f"Ld = {final_ld * 1000:.3f} mH")

# =============== 验证与可视化部分 ===============
import matplotlib.pyplot as plt
from sklearn.metrics import mean_squared_error, mean_absolute_error, r2_score

# 计算预测值
Vd_actual = []
Vq_actual = []
Vd_pred = []
Vq_pred = []

for point in data:
    omega, vd, vq, id_, iq = point
    # 使用估计出的参数计算预测值
    vd_p = Rs * id_ - omega * final_lq * iq
    vq_p = Rs * iq + omega * final_ld * id_ + omega * final_psi
    Vd_actual.append(vd)
    Vq_actual.append(vq)
    Vd_pred.append(vd_p)
    Vq_pred.append(vq_p)

# 计算误差指标
def print_metrics(name, actual, pred):
    mse = mean_squared_error(actual, pred)
    mae = mean_absolute_error(actual, pred)
    r2 = r2_score(actual, pred)
    print(f"{name}误差:")
    print(f"  MSE = {mse:.6f}")
    print(f"  MAE = {mae:.6f}")
    print(f"  R²  = {r2:.3f}")

print("\n验证结果:")
print_metrics("Vd", Vd_actual, Vd_pred)
print_metrics("Vq", Vq_actual, Vq_pred)

# 可视化对比
plt.figure(figsize=(14, 6))

# Vd对比
plt.subplot(1, 2, 1)
plt.scatter(Vd_actual, Vd_pred, c='blue', alpha=0.6, label='Pred vs Actual')
plt.plot([min(Vd_actual), max(Vd_actual)], [min(Vd_actual), max(Vd_actual)], 
         'r--', lw=2, label='Ideal Line')
plt.xlabel('Measured Vd (V)'), plt.ylabel('Predicted Vd (V)')
plt.title('Vd: Prediction vs Measurement')
plt.legend()

# Vq对比
plt.subplot(1, 2, 2)
plt.scatter(Vq_actual, Vq_pred, c='green', alpha=0.6, label='Pred vs Actual')
plt.plot([min(Vq_actual), max(Vq_actual)], [min(Vq_actual), max(Vq_actual)], 
         'r--', lw=2, label='Ideal Line')
plt.xlabel('Measured Vq (V)'), plt.ylabel('Predicted Vq (V)')
plt.title('Vq: Prediction vs Measurement')
plt.legend()

plt.tight_layout()
plt.show()
